/***************************************************************
|			MAIN
|
|This File is the main of the NXBOT proyect, intented to
|configure the firmware of the NXBOT Robots.
|
|There are five tasks to be executed:
|
****************************************************************/

#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdint.h>

#include "FreeRTOS.h"
#include "task.h"


#include "nxbot.h"


/**
 * @brief The handle for the communication task. 
 *
 * This task handles all the communication through the serial port (UART1).
 * It is declared as a global variable, in order to use it in other modules of the firmware.
 */
xTaskHandle commHandle;
/**
 * @brief The handle for the sound task. 
 * 
 * This task is always suspended and only activated by the communication task. 
 * It handles the timing for the musical notes.
 * It is declared as a global variable, in order to use it in other modules of the firmware.
 */
xTaskHandle soundHandle;
/**
 * @brief The handle for the sensor task. 
 *
 * This task is in charge of updating all the data coming from the sensors and organize it in their
 * respective structures declared in structures.h.
 * It is declared as a global variable, in order to use it in other modules of the firmware.
 */
xTaskHandle sensorHandle;

/**
 * @brief The </code>main</code> function. 
 *
 * Initializes all the hardware of the robot and creates the respective tasks to be used; one task is used for motor control, other for sensor  update
 * another one for the sound system (which is always sleeping, and only wakes up when it is needed), and one for the communication role between this
 * system and the embedded one. Finally all the control is given to the scheduler, as the RTOS is implemented as a preemptive one (The scheduler
 * is in charge of assigning CPU resources to each task. A Priority- and Round-Robin strategy is used.
 *
 * The tasks use the global_t structure for sharing information and data. 
 */
int main( void )
{
 
	//default initialization of hardware
	nxbot_init();
	
	
	
	/* definition of global parameters for tasks. The sructure is allocated in memory
	 * A simple definition for globals: All parameters that are shared by all tasks.
	 * Although not all parameters are needed for each task, it is a simple way to share information between tasks.
	 * See include/structures.h for more detailed information.
	 */
	 
	globals_t *globals;
	globals=(globals_t *)pvPortMalloc( sizeof( globals_t ) );

	/* Frequencies of Tasks, Which are measured in ticks.  
	 * Tick frequency is 3000 Hz, it means each tick last for 0.5ms.
	 * See FreeRTOSConfig.h, configTICKRATE_HZ variable for more detailed information.
	 */
	globals->gTimes.intervalMotors = MOTOR_INTERVAL; //250ms
	globals->gTimes.intervalComm = COMM_INTERVAL;
	globals->gTimes.intervalSensors = SENSOR_INTERVAL;
	globals->gTimes.intervalSonars =SONAR_INTERVAL;

	//Initial Velocity of the motors
	globals->gVels.velLeftRef = 0;
	globals->gVels.velRightRef = 0;
	globals->gPiParam.Kp = 20;
	globals->gPiParam.Ki = 20;
	
	//Initial Position of the robot
	globals->gRobotState.posXAct = 0;
	globals->gRobotState.posYAct = 0;

	//now execute tasks
	executeSensorsTask(globals);
	executeCommTask(globals);
	executeMotorsTask(globals);
	executeSoundTask(globals);
	executeSonarTask(globals);
	

	//suspend sound task. It will be enabled by comm task
	vTaskSuspend( soundHandle );

	// Start the scheduler for the tasks	
	vTaskStartScheduler();
	
	return 0;
}
